/*************************************************************
*
* M3D_PRSM.CPP
*
* Prism funcionality
*
* NOTE: 
* Prism is a vertically-aligned box, which base is a 
* clockwise-oriented RECTANGLE.
* After conversion to 3D, the polygon becomes CW 
* oriented (as a 3DXZ polygon), and this is ASSERTed 
* throughout the module
*
* NOTE:
* Prism construction functions are slow, and not intended 
* for real-time use.
* Approximate algorithm is used: rectangle is rotated
* from 0 to 360 to find minimal perimeter enclosing rectangle
*************************************************************/
#include <stdafx.h>

//#include <ap_comm.h>
#include <m3d/m3d.h>

//
// Static functions
//
/*************************************************************
* m3dPRISM::IsBelongPoint()
*
* IN : p     - a point
* RET: TRUE  - point belongs to prism
*      FALSE - otherwise
*
* Check if a point belongs to prism
*************************************************************/
BOOL m3dPRISM::IsBelongPoint (const m3dV *p)
{
   m2dV   v2D;

   // check the Y-range
   if (p->y < yLo - 0.001f ||
       p->y > yHi + 0.001f)  {
      return FALSE;
   }

   // Y-range is OK; check if p belongs to base polygon2D
   m3xzV3DToV2D(p, &v2D);

   return m2dIsBelongPointPoly_A(&polyBase, &v2D, 0.2f);
}
/*************************************************************
* m3dBOX::IsectCyl ()
*    Test isection of cylynder and prizm
*************************************************************/
BOOL m3dPRISM::IsectCyl (m3dV *vFrom, m3dV *dir, float distMax, float radius, m3dV *vClosest, float *dist, int *faceNmb)
{
   m3dPOLY polyList[6];
   int     i;
   m3dV    vTo;

   if (IsBelongPoint(vFrom))  {
      // as ray origin belongs to bbox then ray intersects bbox 
      return TRUE;
   }

   GetSurface(polyList);
   _m3dLineCombine(vFrom, dir, distMax, &vTo);

   for (i = 0; i < 6; i++)  {
      if (m3dDistLSegPoly(vFrom, &vTo, &polyList[i], NULL, NULL, vClosest, dist, NULL, radius)) {
         *faceNmb = i;
         return TRUE;
      }
   }
   return FALSE;
}

/*************************************************************
* m3dPRISM::IsectRay()
*
* Intersect a ray with the outer surface of prism
* NOTE: If point belongs to prism, the intersection data (v, dist)
*       is not computed
*
* IN:  rayOrg/rayDir - ray origin/direction
* OUT: v             - point if intersection (if exists)
*      dist          - distance between rayOrg and v
* RET: M3D_ISECT_NO/M3D_ISECT_YES/M3D_ISECT_BELONG
*************************************************************/
int m3dPRISM::IsectRay (m3dV *rayOrg, m3dV *rayDir, float distMax, m3dV *v, float *dist, int *faceNmb)
{
   m3dPOLY polyList[6];
   int     i;
   m3dV    vert;

   if (IsBelongPoint(rayOrg))  {
      // as ray origin belongs to bbox then ray intersects bbox 
      if (v) {
         *v = *rayOrg;
      }
      if (dist) {
         *dist = 0.f;
      }
      return M3D_ISECT_BELONG;
   }

   GetSurface(polyList);

   for (i = 0; i < 6; i++)  {
      if (m3dIsectRayPlane(rayOrg, rayDir, &polyList[i], &vert))  {
         // ray intersects with the plane of bbox face polygons; check if it intersects
         // directly with the polygon
         if (m3dIsBelongPointPoly(&polyList[i], &vert, NULL))  {
            if (v)  {
               *v = vert;
               *dist = m3dDist(&vert, rayOrg);
               *faceNmb = i;
            }
            return M3D_ISECT_YES;
         }
      }
   }
   
   return M3D_ISECT_NO;
}

/*************************************************************
* m3dPRISM::DistPoint()
* 
* Calculate distance between point and prism
* IN : p        - point
* OUT: vClosest - nearest point on prism (may be NULL)
*      dist     - distance to nearest point (may be NULL)
*      faceNmb  - face index
* RET: M3D_DIST_FOUND/M3D_DIST_NOTFOUND/M3D_DIST_BELONG
*
* NOTE: if 'p' belongs to pBPrizm, vClosest/dist are not filled
*
* NOTE: This function can be significantly sped up (similarly
*       to bbox/ray intersection test)
*************************************************************/
int m3dPRISM::DistPoint (const m3dV *p, float distMax, m3dV *vClosest, float *dist, int *faceNmb)
{
   m3dV    norm;
   m3dPOLY polyList[6];
   int     i, iMin = 0;
   m3dV    v, vMin;
   float   d, dMin;
   BOOL    fFound;

   if (IsBelongPoint(p)) {
      return M3D_DIST_BELONG;
   }
   GetSurface(polyList);

   fFound = FALSE;
   dMin = distMax;
   for (i = 0; i < 6; i++) {
      m3dGetPolyNormal(&polyList[i], &norm);
      if (!m3dDistPointPoly(p, &polyList[i], &norm, FALSE, &v, &d, M3D_INFINITY))  {
         continue;
      }
      if (d < dMin) {
         dMin = d;
         vMin = v;
         iMin = i;
         fFound = TRUE;
      }
   }
   if (!fFound) {
      return M3D_DIST_NOTFOUND;
   }
   if (dist) {
      *dist = dMin;
   }
   if (vClosest) {
      *vClosest = vMin;
   }
   if (faceNmb) {
      *faceNmb = iMin;
   }
   return M3D_DIST_FOUND;
}
//#pragma optimize( "", on ) 


/*************************************************************
* m3dPRISM::Check()
*
* Check the validity of prism
*************************************************************/
BOOL m3dPRISM::Check (void) const
{
   m2dV  v0_1, v1_2, v2_3, v3_0;

   if (!m2dIsPolyConvexCCW(&polyBase)) {
      return FALSE;
   }
   if (yLo > yHi - M3D_EPSILON) {
      return FALSE;
   }

   m2dSubtractVector(&polyBase[1], &polyBase[0], &v0_1);
   m2dSubtractVector(&polyBase[2], &polyBase[1], &v1_2);
   m2dSubtractVector(&polyBase[3], &polyBase[2], &v2_3);
   m2dSubtractVector(&polyBase[0], &polyBase[3], &v3_0);

   if (!m3dIsZero_A(m2dLengthVector(&v0_1) - m2dLengthVector(&v2_3), 0.005f)) {
      return FALSE;
   }
   if (!m3dIsZero_A(m2dLengthVector(&v1_2) - m2dLengthVector(&v3_0), 0.005f)) {
      return FALSE;
   }
   if (m2dLengthVector(&v0_1) < 10.f * M3D_EPSILON) {
      return FALSE;
   }
   m2dNormalize(&v0_1);

   if (m2dLengthVector(&v1_2) < 10.f * M3D_EPSILON) {
      return FALSE;
   }
   m2dNormalize(&v1_2);

   if (m2dLengthVector(&v2_3) < 10.f * M3D_EPSILON) {
      return FALSE;
   }
   m2dNormalize(&v2_3);

   if (m2dLengthVector(&v3_0) < 10.f * M3D_EPSILON) {
      return FALSE;
   }
   m2dNormalize(&v3_0);

   if (!m3dIsZero_A(m2dDotProduct(&v0_1, &v1_2), 1000.f * M3D_EPSILON)) {
      return FALSE;
   }
   if (!m3dIsZero_A(m2dDotProduct(&v1_2, &v2_3), 1000.f * M3D_EPSILON)) {
      return FALSE;
   }
   if (!m3dIsZero_A(m2dDotProduct(&v2_3, &v3_0), 1000.f * M3D_EPSILON)) {
      return FALSE;
   }
   if (!m3dIsZero_A(m2dDotProduct(&v3_0, &v0_1), 1000.f * M3D_EPSILON)) {
      return FALSE;
   }
   return TRUE;
}

/*******************************************************
* m3dPRISM::CalcEnclSph()
*
*******************************************************/
void m3dPRISM::CalcEnclSph (m3dSPHERE *pSph)
{
   m3dV  v;

   v.x = polyBase[0].x;
   v.z = polyBase[0].y;
   v.y = yLo;
   GetCenter(&pSph->org);
   pSph->radius = m3dDist(&v, &pSph->org) + 0.1f;
   return ;
}

/*******************************************************
* m3dPRISM::CheckEnclSph()
*
*******************************************************/
BOOL m3dPRISM::CheckEnclSph (m3dSPHERE *pSph)
{
   int         i;
   m3dV        vertList[8];
   m3dSPHERE   sph;

   sph = *pSph;
   sph.radius += 100*M3D_EPSILON;

   GetVertices(vertList);
   for (i = 0; i < 8; i++) {
      if (!sph.IsBelongPoint(&vertList[i])) {
         return FALSE;
      }
   }
   return TRUE;
}

/*************************************************************
* m3dPRISM::GetCenter()
*
* Get prism center
*************************************************************/
void m3dPRISM::GetCenter (m3dV *center) const
{
   m3dV  v1, v2;

   v1.x = polyBase[0].x;
   v1.z = polyBase[0].y;
   v1.y = yLo;
   v2.x = polyBase[2].x;
   v2.z = polyBase[2].y;
   v2.y = yHi;
   m3dLineCombine(&v1, 0.5f, &v2, 0.5f, center);
   return;
}

/*************************************************************
* m3dPRISM::GetSurface()
*
* OUT: polyList - six polygons comprising the outer surface 
*                 of the prism
*
* Create an array of 6 polygons comprising the outer surface 
* of a prism.
*************************************************************/
void m3dPRISM::GetSurface (m3dPOLY polyList[6])
{
   int   i;

   for (i = 0; i < 6; i++) {
      GetFace(i, &polyList[i]);
   }
   return;
}

/*************************************************************
* m3dPRISM::GetFace()
*
*************************************************************/
void m3dPRISM::GetFace (int faceNmb, m3dPOLY *poly, m3dV *norm)
{
   m3dV  v[8];

   // not implemented yet
   ASSERT(norm == NULL);
   ASSERT(Check());

   GetVertices(v);

   switch (faceNmb) {
      case M3D_VOL_FACE_BACK:
         *m3dPolyVertex(poly, 0) = v[3];
         *m3dPolyVertex(poly, 1) = v[7];
         *m3dPolyVertex(poly, 2) = v[4];
         *m3dPolyVertex(poly, 3) = v[0];
         break;
      case M3D_VOL_FACE_RIGHT:
         *m3dPolyVertex(poly, 0) = v[2];
         *m3dPolyVertex(poly, 1) = v[6];
         *m3dPolyVertex(poly, 2) = v[7];
         *m3dPolyVertex(poly, 3) = v[3];
         break;
      case M3D_VOL_FACE_FRONT:
         *m3dPolyVertex(poly, 0) = v[5];
         *m3dPolyVertex(poly, 1) = v[6];
         *m3dPolyVertex(poly, 2) = v[2];
         *m3dPolyVertex(poly, 3) = v[1];
         break;
      case M3D_VOL_FACE_LEFT:
         *m3dPolyVertex(poly, 0) = v[4];
         *m3dPolyVertex(poly, 1) = v[5];
         *m3dPolyVertex(poly, 2) = v[1];
         *m3dPolyVertex(poly, 3) = v[0];
         break;
      case M3D_VOL_FACE_BOTTOM:
         *m3dPolyVertex(poly, 0) = v[0];
         *m3dPolyVertex(poly, 1) = v[1];
         *m3dPolyVertex(poly, 2) = v[2];
         *m3dPolyVertex(poly, 3) = v[3];
         break;
      case M3D_VOL_FACE_TOP:
         *m3dPolyVertex(poly, 0) = v[7];
         *m3dPolyVertex(poly, 1) = v[6];
         *m3dPolyVertex(poly, 2) = v[5];
         *m3dPolyVertex(poly, 3) = v[4];
         break;
   }
   poly->nVert = 4;

   return;
}
/*************************************************************
* m3dOBB::GetFaceCenter ()
*
*************************************************************/
void m3dPRISM::GetFaceCenter (int faceNmb, m3dV *center)
{
   // not implemented
   ASSERT(FALSE);
   return;
}


/*************************************************************
* m3dPRISM::GetVertices()
*
* OUT: v - array filled with 8 points, corners of prism
*
* Get 8 vertices of the prism
*************************************************************/
void m3dPRISM::GetVertices (m3dV vList[8]) const
{
   int i;

   ASSERT(Check());

   for (i = 0; i < 4; i++)  {
      vList[i].x = polyBase[i].x;
      vList[i].z = polyBase[i].y;
      vList[i].y = yLo;

      vList[i+4].x = polyBase[i].x;
      vList[i+4].z = polyBase[i].y;
      vList[i+4].y = yHi;
   }

   return;
}

/*************************************************************
* m3dPRISM::Expand()
*
* IN : pExpandForw->x - expansion along 0->1 axis forward
*      pExpandForw->y - expansion along Y axis up
*      pExpandForw->z - expansion along 3->0 axis right
*      pExpandBack->x - expansion along 0->1 axis backward
*      pExpandBack->y - expansion along Y axis down
*      pExpandBack->z - expansion along 3->0 axis left
* OUT: pBP       - expanded prism
* RET: TRUE if result is non-degenerate prism
*       (some expansion coefficients can be negative and cause degeneration)
*       But the expansion will be done in any case !
* NOTE: for parallelepipeds ONLY !!!
*************************************************************/
BOOL m3dPRISM::Expand (m3dV *pExpandForw, m3dV *pExpandBack)
{
   m2dV   v0_1, v0_3;
   float  length0_1, length0_3;

   m2dSubtractVector(&polyBase[1], &polyBase[0], &v0_1);
   m2dSubtractVector(&polyBase[3], &polyBase[0], &v0_3);

   length0_1 = m2dLengthVector(&v0_1);
   length0_3 = m2dLengthVector(&v0_3);

   if (length0_1 < 10*M3D_EPSILON && length0_3 < 10*M3D_EPSILON) {
      v0_1 = m2dVUnitX;
      v0_3 = m2dVUnitY;
   } else if (length0_1 < 10*M3D_EPSILON) {
      v0_1.x =   v0_3.y;
      v0_1.y = - v0_3.x;
   } else if (length0_3 < 10*M3D_EPSILON) {
      v0_3.x = - v0_1.y;
      v0_3.y =   v0_1.x;
   } else {
      ; // do nothing
   }

   length0_1 = m2dLengthVector(&v0_1);
   length0_3 = m2dLengthVector(&v0_3);

   m2dNormalize(&v0_1);
   m2dNormalize(&v0_3);

   _m2dLineCombine(&polyBase[0], &v0_1, - pExpandBack->x, &polyBase[0]);
   _m2dLineCombine(&polyBase[3], &v0_1, - pExpandBack->x, &polyBase[3]);

   _m2dLineCombine(&polyBase[1], &v0_1, pExpandForw->x, &polyBase[1]);
   _m2dLineCombine(&polyBase[2], &v0_1, pExpandForw->x, &polyBase[2]);

   _m2dLineCombine(&polyBase[0], &v0_3, - pExpandBack->z, &polyBase[0]);
   _m2dLineCombine(&polyBase[1], &v0_3, - pExpandBack->z, &polyBase[1]);

   _m2dLineCombine(&polyBase[2], &v0_3, pExpandForw->z, &polyBase[2]);
   _m2dLineCombine(&polyBase[3], &v0_3, pExpandForw->z, &polyBase[3]);

   yLo -= pExpandBack->y;
   yHi += pExpandForw->y;

   ASSERT(Check());

   if (length0_1 < - pExpandForw->x - pExpandBack->x) {
      return FALSE;
   }
   if (length0_3 < - pExpandForw->z - pExpandBack->z) {
      return FALSE;
   }
   return TRUE;
}

/*************************************************************
* m3dPRISM::Expand()
* IN : expandForw  - |
*      expandRight - | - expansion parameters (forward, right, up)
*      expandUp    - |
* OUT: expanded prism (this)
* RET: FALSE if the result is degenerate
*************************************************************/
BOOL m3dPRISM::Expand (float expandForw, float expandRight, float expandUp)
{
   m3dV vExpand;
   
   m3dMakeVector(&vExpand, expandForw, expandRight, expandUp);

   return Expand(&vExpand, &vExpand);
}

/************************************************************
* m3dPRISM::Expand()
*
************************************************************/
void m3dPRISM::Expand (float value)
{
   Expand(value, value, value);
   return;
}

/*************************************************************
* m3dPRISM::Calc()
*
* Make bounding prism for vertex list
*
* IN:  vertList - list of points
*      nVert    - number of points in the list
* OUT: this - prism (enclosing point list)
* RET: FALSE if error
*************************************************************/
void m3dPRISM::Calc (m3dV *pointList, int nPoint)
{
   int   angle, optAngle;
   int   i;
   float perimeter, minPerimeter;
   float xMin = 0, xMax = 0, yMin, yMax;

   if (nPoint < 1) {
      ASSERT(FALSE);
      return ;
   }

   // Calculate Ymin, Ymax values
   yMin = yMax = pointList->y;
   for (i = 1; i < nPoint; i++) {
      yMin = min(yMin, pointList[i].y);
      yMax = max(yMax, pointList[i].y);
   }
   yLo = yMin;
   yHi = yMax;

   optAngle = 0;
   minPerimeter = M3D_INFINITY;
   for (angle = 0; angle < 90; angle += 5) {
      // Generate all bounding rects and calculate their perimeters
      perimeter = _MakeBPrismRect(pointList, nPoint, (float)angle, &xMin, &xMax, &yMin, &yMax);
      if (perimeter < minPerimeter) {
         minPerimeter = perimeter;
         optAngle = angle;
      }
   }
   // Use optimal angle to make prism
   _MakeBPrismRect(pointList, nPoint, (float)optAngle, &xMin, &xMax, &yMin, &yMax);

   //
   // Now: xMin/max, yMin/max contain rectangle coordinates
   //      in rotated CS
   // Rotate them again to main prism
   //
   _MakeBPrismPolyBase((float)optAngle, xMin, xMax, yMin, yMax);

   return ;
}

/*************************************************************
* m3dPRISM::_MakeBPrismRect()
*
* Make bounding rectangle for 'angle'
*
* IN : vertList - vertex list
*      nVert    - number of vertives
*      angle    - angle for rect rotation
* RET: perimeter of the prism for the given angle
*************************************************************/
float m3dPRISM::_MakeBPrismRect (m3dV *vertList, int nVert, float angle,
                                 float *xMin, float *xMax, float *yMin, float *yMax)
{
   m3dMATR  rotMatr;
   m3dV     vRes;
   int      i;

   // Make transformation matrix for current angle
   rotMatr.MakeRotY(angle);

   // Calculate min/max values for all points
   for (i = 0; i < nVert; i++) {
      rotMatr.TransformPoint(vertList+i, &vRes);

      if (i == 0) {
         *xMin = *xMax = vRes.x;
         *yMin = *yMax = vRes.z;
      } else {
         *xMin = min(*xMin, vRes.x);
         *xMax = max(*xMax, vRes.x);
         *yMin = min(*yMin, vRes.z);
         *yMax = max(*yMax, vRes.z);
      }
   }
   if (nVert == 1) {
      *xMin -= 0.1f;
      *yMin -= 0.1f;
      *xMax += 0.1f;
      *yMax += 0.1f;
   }

   return (*xMax - *xMin)+(*yMax - *yMin);
}

/*************************************************************
* m3dPRISM::_MakeBPrismPolyBase()
*
* Make bounding prism polyBase using optimal angle
* IN  : angle
* OUT : this->polyBase
*************************************************************/
void m3dPRISM::_MakeBPrismPolyBase (float angle,
                                    float xMin, float xMax, float yMin, float yMax)
{
   m3dMATR  rotMatr;
   m3dV     vOld, vRes;

   // Make transformation matrix for current angle (back rotate)
   rotMatr.MakeRotY(-angle);

   // Transform all points of optimal rect using transformation matrix
   polyBase.nVert = 4;
   vOld.x = xMin - M3D_EPSILON;
   vOld.y = 0.f;
   vOld.z = yMin - M3D_EPSILON;
   rotMatr.TransformPoint(&vOld, &vRes);
   m3xzV3DToV2D(&vRes, polyBase.vert);

   vOld.x = xMax + M3D_EPSILON;
   vOld.y = 0.f;
   vOld.z = yMin - M3D_EPSILON;
   rotMatr.TransformPoint(&vOld, &vRes);
   m3xzV3DToV2D(&vRes, polyBase.vert+1);

   vOld.x = xMax + M3D_EPSILON;
   vOld.y = 0.f;
   vOld.z = yMax + M3D_EPSILON;
   rotMatr.TransformPoint(&vOld, &vRes);
   m3xzV3DToV2D(&vRes, polyBase.vert+2);

   vOld.x = xMin - M3D_EPSILON;
   vOld.y = 0.f;
   vOld.z = yMax + M3D_EPSILON;
   rotMatr.TransformPoint(&vOld, &vRes);
   m3xzV3DToV2D(&vRes, polyBase.vert+3);

   return;
}
/************************************************************
* m3dPRISM::QualPlane ()
*
************************************************************/
int m3dPRISM::QualPlane (m3dV *planeOrg, m3dV *planeNorm)
{
   m3dV   vList[8];

   GetVertices(vList);
   return m3dQualVListPlane(planeOrg, planeNorm, 8, vList);
}

/***************************************************************
* m3dPRISM::GetMatrW2L()
*
***************************************************************/
void m3dPRISM::GetMatrW2L (m3dMATR *pMatrW2L)
{
   m3dV  vX, vZ, org;

   vX.x = polyBase.vert[1].x - polyBase.vert[0].x;
   vX.z = polyBase.vert[1].y - polyBase.vert[0].y;
   vX.y = 0.f;

   vZ.x = polyBase.vert[3].x - polyBase.vert[0].x;
   vZ.z = polyBase.vert[3].y - polyBase.vert[0].y;
   vZ.y = 0.f;

   org.x = polyBase.vert[0].x;
   org.z = polyBase.vert[0].y;
   org.y = yLo;

   m3dNormalize(&vX);
   m3dNormalize(&vZ);

   pMatrW2L->MakeWCS2LCS(&org, &vX, &m3dVUnitY, &vZ);
   return;
}

/***************************************************************
* m3dPRISM::GetMatrL2W()
*
***************************************************************/
void m3dPRISM::GetMatrL2W (m3dMATR *pMatrL2W)
{
   m3dMATR  matrW2L;

   GetMatrW2L(&matrW2L);
   matrW2L.Invert(pMatrL2W);
   return;

}

/***************************************************************
* m3dPRISM::ConvToOBB()
*
***************************************************************/
void m3dPRISM::ConvToOBB (m3dOBB *pOBB)
{
   m2dV *vertList = polyBase.vert;

   pOBB->GetOrg()->x = vertList[0].x;
   pOBB->GetOrg()->z = vertList[0].y;
   pOBB->GetOrg()->y = yLo;

   pOBB->GetDirX()->x = vertList[1].x - vertList[0].x;
   pOBB->GetDirX()->z = vertList[1].y - vertList[0].y;
   pOBB->GetDirX()->y = 0.f;
   pOBB->GetSizeX() = m3dLengthVector(pOBB->GetDirX());
   m3dNormalize(pOBB->GetDirX());

   *pOBB->GetDirY() = m3dVUnitY;
   pOBB->GetSizeY() = yHi;

   pOBB->GetDirZ()->x = vertList[3].x - vertList[0].x;
   pOBB->GetDirZ()->y = vertList[3].y - vertList[0].y;
   pOBB->GetDirZ()->y = 0.f;
   pOBB->GetSizeZ() = m3dLengthVector(pOBB->GetDirZ());
   m3dNormalize(pOBB->GetDirZ());
   return;
}

//
// End-of-file M3D_PRSM.CPP
//
